#include "string.h"
#include "system_veri.h"
#include "stdio.h"
#include "insn.h"
#include "hbird_sdk_soc.h"

unsigned int global_sigma = 0;
unsigned int global_u42a = 0;
unsigned int global_gamma = 0;
unsigned int global_psy = 0;
unsigned int mrt_cfg_wm = 0;
unsigned int mrt_cfg_on = 0;
unsigned int mrt_cfg_ur = 0;
unsigned int mrt_finish_flag = 0;
unsigned int mrt_type = 0;

const unsigned int sigma_th = 6710886;
const unsigned int gamma_th = 12582912;
const unsigned int psy_th = 32212255;
const unsigned int u42a_th = 334604796;

void adc_init(){
	gpio_enable_output(GPIOB, SOC_ADC_CTRL_GPIO_MSK | SOC_ADC_ENA_GPIO_MSK);
	gpio_write(GPIOB, SOC_ADC_CTRL_GPIO_MSK | SOC_ADC_ENA_GPIO_MSK, GPIO_BIT_ALL_ZERO);
	delay_1ms(15);
	return ;
}

void gpio_init(){
	gpio_enable_input(GPIOA, SOC_MODESEL_GPIO_MSK);
	gpio_enable_output(GPIOA, SOC_SYSSTATUS_GPIO_MSK | SOC_MRTRESULT_GPIO_MSK);
	config();
	delay_1ms(15);
	return ;
}

void config(){
	int32_t gpio_a;
	gpio_a = gpio_read(GPIOA,SOC_MODESEL_GPIO_MSK);
	mrt_cfg_on = ((gpio_a & SOC_SWC0_GPIO_MSK)>>SOC_SWC0_GPIO_OFS)&0x1;
	mrt_cfg_wm = (gpio_a & SOC_SWC1_GPIO_MSK)>>SOC_SWC1_GPIO_OFS;
	mrt_cfg_ur = (gpio_a & (SOC_SWC4_GPIO_MSK|SOC_SWC5_GPIO_MSK))>>SOC_SWC5_GPIO_OFS;
	gpio_write(GPIOA, SOC_LED0_GPIO_MSK, mrt_cfg_on);
	gpio_write(GPIOA, SOC_LED1_GPIO_MSK, mrt_cfg_wm);
}

void adc_select_type_and_start(int i){
	gpio_write(GPIOB, SOC_ADC_ENA_GPIO_MSK, GPIO_BIT_ALL_ZERO);

	gpio_write(GPIOB, SOC_ADC_TYPE0_GPIO_MSK, i & 0x1);
	gpio_write(GPIOB, SOC_ADC_TYPE1_GPIO_MSK, i & 0x2);
	gpio_write(GPIOB, SOC_ADC_TYPE2_GPIO_MSK, i & 0x4);
	gpio_write(GPIOB, SOC_ADC_TYPE3_GPIO_MSK, i & 0x8);

	gpio_write(GPIOB, SOC_ADC_ENA_GPIO_MSK, GPIO_BIT_ALL_ONE);
	return ;
}

void adc_stop(){
	gpio_write(GPIOB, SOC_ADC_CTRL_GPIO_MSK | SOC_ADC_ENA_GPIO_MSK, GPIO_BIT_ALL_ZERO);
	return ;
}

void plic_mrt_finish_handler(void)
{
	int mask;
	printf("Enter MRT finish irq\n");
	global_u42a = custom_mrt_rdstat(1);
	global_sigma = custom_mrt_rdstat(2);
	global_gamma = custom_mrt_rdstat(3);
	global_psy = custom_mrt_rdstat(4);
	mask = gpio_clear_interrupt(GPIOB);
	mrt_finish_flag = 1;
	// run Decision Tree
	if (global_sigma > sigma_th){
		if(mrt_cfg_wm){
			unsigned int abs_psy;
			unsigned int psy_negtive = global_psy & 0x80000000;
			abs_psy = psy_negtive ? ((global_psy^0x7fffffff)+1)&0x7fffffff : global_psy;

			if(abs_psy > psy_th){
				if(psy_negtive) mrt_type = 4;
				else mrt_type = 5;
			}
			else{
				if(global_gamma > gamma_th) mrt_type = 8;
				else mrt_type = 10;
			}
		}
		else{
			mrt_type = 12;
		}
	}
	else{
		if(global_u42a > u42a_th) mrt_type = 2;
		else mrt_type = 1;
	}
	// set GPIOA MRT_RESULT value
	gpio_write(GPIOA, SOC_MRTRESULT_GPIO_MSK, GPIO_BIT_ALL_ZERO);
	gpio_write(GPIOA, (mrt_type&0x0f)<<SOC_LED5_GPIO_OFS, GPIO_BIT_ALL_ONE);

	return ;
}

void mrt_intr_init(){
	int32_t returnCode;
	gpio_enable_input(GPIOB, SOC_MRT_INTR_GPIO_MSK);
	gpio_enable_interrupt(GPIOB, SOC_MRT_INTR_GPIO_MSK, GPIO_INT_RISE);
	returnCode = PLIC_Register_IRQ(PLIC_GPIOB_IRQn, 1, plic_mrt_finish_handler);
	return ;
}


void print_fixed_point(unsigned int fp_num, int bw, int frac_bw, int usign, int pricision){
	int p,i;
	char q[20];
	uint64_t t;
	unsigned int u = fp_num;
	uint64_t t_p;
	uint64_t e10 = 10 << frac_bw;
	p = 0;
	i = 0;
	if (usign && (fp_num & (1<<(bw-1)))){
		int mask = 0x7fffffff>>(32-bw);
		u = ((u^mask)+1) & mask;
		printf("-");
	}
	t = u;
	p = t >> frac_bw;
	t_p = p << frac_bw;
	t = (t-t_p)*10;
	while(i<pricision){
		t = t*10;
		t_p = t/e10;
		q[i] = '0' + t_p;
		t = t - t_p*e10;
		i++;
	}
	q[i]=0;
	printf("%d.%s",p,q);
}
